#include "step_motor_usart.h"
#include "motor_task.h"
//角度回传处理函数
static void Receive_TO_Ture(uint8_t *rx_buf,motor_measure_t *pitch_data, motor_measure_t *yaw_data);
static void USART6_Step_Config(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num);

//陀螺仪数据
//static fp32 Angle_Z;
//static fp32 Gyro_Z;
static motor_measure_t pitch_gimal_motor;
static motor_measure_t yaw_gimal_motor;
//接收原始数据，为11个字节
static uint8_t motor_rx_buf[2][STEP_RX_BUF_NUM];


/**
  * @brief  USART5 GPIO 配置,工作模式配置。115200 9-E-1 
  * @param  无
  * @retval 无
  */


void step_usart_init(void)
{
	//数据指针-电机指向设置结构体
//	Data_receive->motor_msg = get_step_motor_point();
	pitch_gimal_motor.real_one = get_pitch_real_point();
	yaw_gimal_motor.real_one = get_yaw_real_point();
	
	USART6_Step_Config(motor_rx_buf[0], motor_rx_buf[1], STEP_RX_BUF_NUM);
}




static void USART6_Step_Config(uint8_t *rx1_buf, uint8_t *rx2_buf, uint16_t dma_buf_num)         //RX-PG9  TX-PG14
{
	//DMA2数据流1通道5
	/* -------------- Enable Module Clock Source ----------------------------*/
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOG | RCC_AHB1Periph_DMA2, ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART6, ENABLE);

	GPIO_PinAFConfig(GPIOG, GPIO_PinSource9, GPIO_AF_USART6); 	// PC12  usart3_tx
	GPIO_PinAFConfig(GPIOG, GPIO_PinSource14, GPIO_AF_USART6);  // PD2   usart3_rx
	GPIO_InitTypeDef GPIO_InitStructure;
	USART_InitTypeDef USART_InitStructure;

	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;
	GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
	GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
	GPIO_Init(GPIOG, &GPIO_InitStructure);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14;
	GPIO_Init(GPIOG, &GPIO_InitStructure);

	USART_DeInit(USART6);

	USART_InitStructure.USART_BaudRate = 115200;
	USART_InitStructure.USART_WordLength = USART_WordLength_8b;
	USART_InitStructure.USART_StopBits = USART_StopBits_1;
	USART_InitStructure.USART_Parity = USART_Parity_No;
	USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
	USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
	USART_Init(USART6, &USART_InitStructure);

//	USART_DMACmd(USART6, USART_DMAReq_Rx, ENABLE);

	// USART_ClearFlag(USART3, USART_FLAG_RXNE);
	// USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
//	USART_ClearFlag(USART6, USART_FLAG_IDLE);
//	USART_ITConfig(USART6, USART_IT_IDLE, ENABLE);

	USART_Cmd(USART6, ENABLE);

	/* ------------------ Configure USART3 NVIC ----------------------- ------------*/
//	NVIC_InitTypeDef NVIC_InitStructure;
//	NVIC_InitStructure.NVIC_IRQChannel = USART6_IRQn;
//	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = STEP_NVIC;
//	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
//	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
//	NVIC_Init(&NVIC_InitStructure);

//	// DMA1 stream0 ch4
//	/* ----------------------- Configure DMA -----------------------------------*/
//	DMA_InitTypeDef DMA_InitStructure;
//	DMA_DeInit(DMA2_Stream1);

//	DMA_InitStructure.DMA_Channel = DMA_Channel_5;
//	DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t) & (USART6->DR);
//	DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)rx1_buf;
//	DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
//	DMA_InitStructure.DMA_BufferSize = dma_buf_num;
//	DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
//	DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
//	DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
//	DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte;
//	DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
//	DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
//	DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
//	DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_1QuarterFull;
//	DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
//	DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
//	DMA_Init(DMA2_Stream1, &DMA_InitStructure);

//	DMA_DoubleBufferModeConfig(DMA2_Stream1, (uint32_t)rx2_buf, DMA_Memory_0); // 双缓冲模式
//	DMA_DoubleBufferModeCmd(DMA2_Stream1, ENABLE);
//	DMA_Cmd(DMA2_Stream1, ENABLE);
}



void USART6_IRQHandler(void)
{

	
	if (USART_GetITStatus(USART6, USART_IT_RXNE) != RESET)
     {
         USART_ReceiveData(USART6);
     }
     else if (USART_GetITStatus(USART6, USART_IT_IDLE) != RESET)
     {
         static uint16_t this_time_rx_len = 0;
         USART_ReceiveData(USART6);

         if(DMA_GetCurrentMemoryTarget(DMA2_Stream1) == 0)
         {
             //重新设置DMA
             DMA_Cmd(DMA2_Stream1, DISABLE);
             this_time_rx_len = STEP_RX_BUF_NUM - DMA_GetCurrDataCounter(DMA2_Stream1);
             DMA_SetCurrDataCounter(DMA2_Stream1, STEP_RX_BUF_NUM);
             DMA2_Stream1->CR |= DMA_SxCR_CT;
             //清DMA中断标志
             DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF5 | DMA_FLAG_HTIF5);
             DMA_Cmd(DMA2_Stream1, ENABLE);
			 if(this_time_rx_len==6)
				Receive_TO_Ture(motor_rx_buf[0], &pitch_gimal_motor, &yaw_gimal_motor);
             
         }
         else
         {
             //重新设置DMA
             DMA_Cmd(DMA2_Stream1, DISABLE);
             this_time_rx_len = STEP_RX_BUF_NUM - DMA_GetCurrDataCounter(DMA2_Stream1);
             DMA_SetCurrDataCounter(DMA2_Stream1, STEP_RX_BUF_NUM);
             DMA2_Stream1->CR &= ~(DMA_SxCR_CT);
             //清DMA中断标志
             DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF5 | DMA_FLAG_HTIF5);
             DMA_Cmd(DMA2_Stream1, ENABLE);
                 //处理陀螺仪数据 
			 if(this_time_rx_len==6)
				Receive_TO_Ture(motor_rx_buf[1], &pitch_gimal_motor, &yaw_gimal_motor);
         }
     }
	
}



static void Receive_TO_Ture(uint8_t *rx_buf,motor_measure_t *pitch_data, motor_measure_t *yaw_data)
{
	int sum=0;
	int i;
	
	
	if(rx_buf[5]!=STEP_END_BYTE)
		return;
	else {
		
		rx_buf[5]=0;
		switch(rx_buf[0]) {
			case PITCH_FRAME_IDENTI:           //接收到PITCH
				pitch_data->last_ecd = pitch_data->ecd;
				pitch_data->ecd = ((int32_t)rx_buf[1]<<24) | ((int32_t)rx_buf[2]<<16) | ((int32_t)rx_buf[3]<<8) | ((int32_t)rx_buf[4]<<0);
	
		//电机转过的实际弧度为编码器值*比例系数

			break;
			
			case YAW_FRAME_IDENTI:
				yaw_data->last_ecd = yaw_data->ecd;
				yaw_data->ecd = ((int32_t)rx_buf[1]<<24) | ((int32_t)rx_buf[2]<<16) | ((int32_t)rx_buf[3]<<8) | ((int32_t)rx_buf[4]<<0);
			

			break;
			
			default:
				break;
			
			
			
			
		}
		
		rx_buf[0]=0;
		rx_buf[1]=0;
		rx_buf[2]=0;
		rx_buf[3]=0;
		rx_buf[4]=0;
		
	}
	
	
	
}

void get_pitch_data(void)
{
	static unsigned char send[3] = {0x01, 0x36, 0x6b};
	Usart_SendArray(USART6,send,3);
}


void get_yaw_data(void)
{
	static unsigned char send[3] = {0x02, 0x36, 0x6b};
	Usart_SendArray(USART6,send,3);
}


//0~0x4ff
void set_pitch_vice(char dir, int16_t vice)
{
	unsigned char send[6] = {0x01, 0xf6, 0x00, 0x00, 0xff, 0x6b};
	send[2] |= (dir<<7);
	send[2] |= ((vice&0x04ff)>>8);
	send[3] |= vice&0xff;
	Usart_SendArray(USART6,send,6);
}

//0~0x4ff
void set_yaw_vice(char dir, int16_t vice)
{
	unsigned char send[6] = {0x02, 0xf6, 0x00, 0x00, 0xff, 0x6b };
	send[2] |= (dir<<7);
	send[2] |= ((vice&0x4ff)>>8);
	send[3] |= vice&0xff;
	Usart_SendArray(USART6,send,6);
}

const motor_measure_t *get_pitch_data_point(void)
{
	return &pitch_gimal_motor;
}

const motor_measure_t *get_yaw_data_point(void)
{
	return &yaw_gimal_motor;
}



